buffer.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Wolf includes
#include "core/common/wolf.h"
 
namespace wolf
{
template <typename T>
class Buffer
{
  public:
    typedef typename std::map<TimeStamp, T>::iterator       Iterator;       // buffer iterator
    typedef typename std::map<TimeStamp, T>::const_iterator ConstIterator;  // buffer iterator
 
    Buffer(){};
    ~Buffer(void){};
 
    T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
 
    ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
    Iterator      selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
    T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
 
    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
 
    T selectFirst() const;
 
    T selectLast() const;
 
    SizeStd size(void) const;
 
    void emplace(const TimeStamp& _time_stamp, const T& _element);
 
    const std::map<TimeStamp, T>& getContainer() const;
    std::map<TimeStamp, T>&       getContainer();
 
    void removeUpTo(const TimeStamp& _time_stamp);
 
    void removeUpToLower(const TimeStamp& _time_stamp);
 
    void clear();
 
    bool empty() const;
 
    static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
                                   const TimeStamp& _time_stamp2,
                                   const double&    _time_tolerance);
 
    static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
                                         const double&    _time_tolerance1,
                                         const TimeStamp& _time_stamp2,
                                         const double&    _time_tolerance2);
 
  protected:
    std::map<TimeStamp, T> container_;  // Main buffer container
};
 
}  // namespace wolf
 
#include "core/common/time_stamp.h"
 
namespace wolf
{
template <typename T>
inline typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
                                                                   const double&    _time_tolerance) const
{
    Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
 
    bool prev_exists = (post != container_.begin());
    bool post_exists = (post != container_.end());
 
    bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
 
    if (prev_exists)
    {
        Buffer<T>::ConstIterator prev = std::prev(post);
 
        bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
 
        if (prev_ok && !post_ok)
            return prev;
 
        else if (!prev_ok && post_ok)
            return post;
 
        else if (prev_ok && post_ok)
        {
            if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
                return post;
            else
                return prev;
        }
    }
    else if (post_ok)
        return post;
 
    return container_.end();
}
 
template <typename T>
inline typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
                                                              const double&    _time_tolerance)
{
    Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
 
    bool prev_exists = (post != container_.begin());
    bool post_exists = (post != container_.end());
 
    bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
 
    if (prev_exists)
    {
        Buffer<T>::Iterator prev = std::prev(post);
 
        bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
 
        if (prev_ok && !post_ok)
            return prev;
 
        else if (!prev_ok && post_ok)
            return post;
 
        else if (prev_ok && post_ok)
        {
            if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
                return post;
            else
                return prev;
        }
    }
    else if (post_ok)
        return post;
 
    return container_.end();
}
 
template <typename T>
inline T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
    if (container_.empty()) return T();  // nullprt in case of T being a pointer
 
    auto it = selectIterator(_time_stamp, _time_tolerance);
 
    // end is returned from selectIterator if an element of the buffer complying with the time stamp
    // and time tolerance has not been found
    if (it != container_.end())
    {
        return it->second;
    }
 
    return T();  // nullprt in case of T being a pointer
}
 
template <typename T>
inline T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
    // There is no element
    if (container_.empty()) return T();  // nullprt in case of T being a pointer
 
    // Checking on begin() since elements are ordered in time
    // Return first element if is older than time stamp
    if (container_.begin()->first < _time_stamp) return container_.begin()->second;
 
    // Return first element if despite being newer, it is within the time tolerance
    if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance)) return container_.begin()->second;
 
    // otherwise return nullptr (no element before the provided ts or within the tolerance was found)
    return T();  // nullprt in case of T being a pointer
}
 
template <typename T>
inline T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
{
    // There is no element
    if (container_.empty()) return T();  // nullprt in case of T being a pointer
 
    // Checking on rbegin() since elements are ordered in time
    // Return last element if is newer than time stamp
    if (container_.rbegin()->first > _time_stamp) return container_.rbegin()->second;
 
    // Return last element if despite being older, it is within the time tolerance
    if (checkTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
        return container_.rbegin()->second;
 
    // otherwise return nullptr (no element after the provided ts or within the tolerance was found)
    return T();  // nullprt in case of T being a pointer
}
 
template <typename T>
inline T Buffer<T>::selectFirst() const
{
    // There is no element
    if (container_.empty()) return T();  // nullprt in case of T being a pointer
 
    // Returning first map element
    return container_.begin()->second;
}
 
template <typename T>
inline T Buffer<T>::selectLast() const
{
    // There is no element
    if (container_.empty()) return T();  // nullprt in case of T being a pointer
 
    // Returning last map element
    return container_.rbegin()->second;
}
 
template <typename T>
inline void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
    container_.emplace(_time_stamp, _element);
}
 
template <typename T>
inline const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
{
    return container_;
}
 
template <typename T>
inline std::map<TimeStamp, T>& Buffer<T>::getContainer()
{
    return container_;
}
 
template <typename T>
inline void Buffer<T>::clear()
{
    container_.clear();
}
 
template <typename T>
inline bool Buffer<T>::empty() const
{
    return container_.empty();
}
 
template <typename T>
inline SizeStd Buffer<T>::size(void) const
{
    return container_.size();
}
 
template <typename T>
inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
{
    Buffer::Iterator post = container_.upper_bound(_time_stamp);
    container_.erase(container_.begin(), post);  // erasing by range
}
 
template <typename T>
inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
{
    Buffer::Iterator post = container_.lower_bound(_time_stamp);
    container_.erase(container_.begin(), post);  // erasing by range
}
 
template <typename T>
inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
                                                const double&    _time_tolerance1,
                                                const TimeStamp& _time_stamp2,
                                                const double&    _time_tolerance2)
{
    double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
    double time_tol  = std::min(_time_tolerance1, _time_tolerance2);
    bool   pass      = time_diff <= time_tol;
    return pass;
}
 
template <typename T>
inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
                                          const TimeStamp& _time_stamp2,
                                          const double&    _time_tolerance)
{
    double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
    bool   pass      = time_diff <= _time_tolerance;
    return pass;
}
 
// SPECIFIC BUFFERS
// fwd declarations
class VectorComposite;
 
class BufferFrame : public Buffer<FrameBasePtr>
{
  public:
    void emplace(FrameBasePtr _frm);
};
 
class BufferCapture : public Buffer<CaptureBasePtr>
{
  public:
    void emplace(CaptureBasePtr _cap);
};
 
class BufferVectorComposite : public Buffer<VectorComposite>
{
};
 
}  // namespace wolf